Hardware interface for sending motion primitives to the robot via the Instruction Executor instead of trajectories #1341
Conversation
…overflow error when launching
…calls (and moved some more code to the helper header)
…tware_version_major_>= 5) in transform_force_torque() since ur_driver_ is not available in the ur_state_helper.hpp
… case to fit the rest of the repo
…31415/ros2_motion_primitives_ur_driver_pkg to the Universal_Robots_ROS2_Driver
… the hardware interface can receive a new motion primitive — replaces the previous, more complex handling via execution_status
…unch.py and added if else for driver_type to load the right ros2_control.xacro file in ur.urdf.xacro
to motion_primitives_forward_controller::motion_primitives_forward_controller
…d to revert the change after the PRs are merged.
… multiple callbacks.
…oupdate Auto-update pre-commit hooks
…motion_primitive/q1)
| url: https://github.com/b-robotized-forks/ros2_controllers | ||
| version: motion_primitive_forward_controller | ||
| # url: https://github.com/ros-controls/ros2_controllers | ||
| # version: master |
There was a problem hiding this comment.
Just for reference. These will have to be applied once everything is merged upstream.
| url: https://github.com/b-robotized-forks/ros2_controllers | |
| version: motion_primitive_forward_controller | |
| # url: https://github.com/ros-controls/ros2_controllers | |
| # version: master | |
| url: https://github.com/ros-controls/ros2_controllers | |
| version: master |
| std::lock_guard<std::mutex> guard(moprim_cmd_mutex_); | ||
| if (!new_moprim_cmd_available_) { | ||
| // Copy command to thread-safe buffer | ||
| pending_moprim_cmd_ = hw_moprim_commands_; | ||
| new_moprim_cmd_available_ = true; | ||
| resetMoprimCmdInterfaces(); | ||
| } |
There was a problem hiding this comment.
To avoid this mutex, you could use a realtime_tools::LockFreeQueue for pushing the commands to the execution thread.
There was a problem hiding this comment.
I replaced the moprim_cmd_mutex_ with realtime_tools::LockFreeSPSCQueue<std::array<double, 25>, 1> and also changed the command and state interfaces from std::vector to std::array<double, 25>.
@urfeex does that look good to you?
There was a problem hiding this comment.
From a first glance this looks good, I'll have a closer look either tonight or tomorrow morning.
There was a problem hiding this comment.
Couldn't the new_moprim_cmd_available_ member be replaced by checking whether the queue is empty in asyncMoprimCmdThread? We can always fill the queue in handleMoprimCommands and pop from the queue whenever there is data available. From my understanding this would exactly fit the usecase mentioned in the queue's docstring.
|
@mathias31415 regarding b4f4dec, please let's keep that back for a follow-up PR. |
…truction_executor_ execution is done
…::array<double, 25>, 1> → therefore changed command and state interfaces from std::vector<double> to std::array<double, 25>.
| } | ||
|
|
||
| // 0: Unconstrained mode, 1: Fixed mode | ||
| // (https://tools.pages.cba.mit.edu/Universal_Robotics_UR10_Robot_Arm/scriptManual-3.5.4.pdf) |
There was a problem hiding this comment.
| // (https://tools.pages.cba.mit.edu/Universal_Robotics_UR10_Robot_Arm/scriptManual-3.5.4.pdf) | |
| // (https://www.universal-robots.com/manuals/EN/HTML/SW5_22/Content/prod-scriptmanual/all_scripts/movec_pose_via_pose_toa1.htm) |
| @@ -323,7 +326,7 @@ def generate_launch_description(): | |||
| declared_arguments.append( | |||
| DeclareLaunchArgument( | |||
| "headless_mode", | |||
| default_value="false", | |||
| default_value="true", | |||
This PR extends the
ur_robot_driverpackage with a hardware interface for sending motion primitives to the robot via the Instruction Executor instead of traditional trajectory execution.To reuse the state interface functionalities of the original hardware interface in the new
motion_primitive_ur_driver, these functionalities have been refactored into a shared helper file:stateinterface_helper.hpp.The implementation uses the
motion_primitives_forward_controller, which is planned to be integrated into theros2_controllersrepository. (ros-controls/ros2_controllers#1636)Motion primitives are received by the controller using the
MotionPrimitive.msgfrom theindustrial_robot_motion_interfacespackage. A modified version of this message is used here, which includes additional helper types:STOP_MOTION: Interrupts current robot motion and clears the motion queue.MOTION_SEQUENCE_STARTandMOTION_SEQUENCE_END: Define a sequence block—primitives between these markers are grouped and executed as a single, blended motion sequence. This enables smooth transitions between individual primitives, unlike executing them one by one.This implementation also depends on a pending Pull Request in the
Universal_Robots_Client_Library. This PR extends the Instruction Executor with support formoveccommands, which is required for executing circular Cartesian motions within this driver.I'd appreciate any feedback or suggestions – thanks in advance!